/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author Zhang Wanjie                                             */

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Pose.h>
#include <tf/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <control_msgs/GripperCommandAction.h>
#include <dynamic_reconfigure/server.h>
#include <visualization_msgs/Marker.h>
#include "wpv4_behaviors/mani_planConfig.h"
#include <math.h>

// 抓取参数调节（单位：米）
static float grab_y_offset = 0.0f;                  // 机器人对准物品，横向位移偏移量
static geometry_msgs::Pose target_pose;             // 获取规划目标
static ros::Publisher result_pub;                   // 执行结果发布
static std_msgs::String result_msg;                 // 执行结果消息
static tf::StampedTransform transform;
static visualization_msgs::Marker plan_marker;
static ros::Publisher plan_marker_pub;

#define STEP_READY        0
#define STEP_PLAN         1
#define STEP_EXEC         2
int step = STEP_READY;

bool new_plan = false;

void PlanCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
    if(step != STEP_READY)
    {
        result_msg.data = "buzy";
        result_pub.publish(result_msg);
        return;
    }
    // 规划目标
    target_pose = *msg;
    ROS_WARN("[PlanCallback] x = %.2f y= %.2f ,z= %.2f " ,target_pose.position.x, target_pose.position.y, target_pose.position.z);
    new_plan = true;
}

void CfgManiPlanCallback(wpv4_behaviors::mani_planConfig &config, uint32_t level) 
{
    if(step != STEP_READY)
    {
        return;
    }
    target_pose.position.x = config.target_x;
    target_pose.position.y = config.target_y;
    target_pose.position.z = config.target_z;
    ROS_WARN("动态设置规划目标 (%.2f , %.2f , %.2f)", target_pose.position.x, target_pose.position.y, target_pose.position.z);
    // 机械臂末端朝向
    tf::Quaternion quat;
    // 手爪初始姿态（全部为0为朝上）,函数三个参数分别为滚转,俯仰和偏转角,单位为弧度
    quat.setRPY(config.target_roll, config.target_pitch, config.target_yaw);
    // 将欧拉角旋转量转换成四元数表达
    transform.setRotation(quat);
    target_pose.orientation.x= transform.getRotation().getX();
    target_pose.orientation.y = transform.getRotation().getY();
    target_pose.orientation.z = transform.getRotation().getZ();
    target_pose.orientation.w = transform.getRotation().getW();
    new_plan = true;
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "wpv4_plan_server");

    ros::NodeHandle nh;
    ros::Subscriber plan_sub = nh.subscribe("/wpv4/mani_plan", 1 , PlanCallback);
    result_pub =  nh.advertise<std_msgs::String>("/wpv4/plan_result", 10);
    plan_marker_pub = nh.advertise<visualization_msgs::Marker>("/wpv4/plan_marker", 10);

    dynamic_reconfigure::Server<wpv4_behaviors::mani_planConfig> mani_plan_server;
    dynamic_reconfigure::Server<wpv4_behaviors::mani_planConfig>::CallbackType mani_plan_func;

    mani_plan_func = boost::bind(&CfgManiPlanCallback, _1, _2);
    mani_plan_server.setCallback(mani_plan_func);

    actionlib::SimpleActionClient<control_msgs::GripperCommandAction> acGripper("wpm2_gripper_controller/gripper_command", true);
    ros::AsyncSpinner spinner(1);
    spinner.start();
    // 等待运动规划服务启动
    acGripper.waitForServer();
    // 获取手臂规划组
    moveit::planning_interface::MoveGroupInterface group("arm");

    ros::Rate r(30);
    while(nh.ok())
    {
        if(new_plan == true)
        {
            // 发送进度消息
            result_msg.data = "plan";
            result_pub.publish(result_msg);
            step = STEP_PLAN;
            // 在rviz里显示
            plan_marker.header.frame_id = "base_footprint";
            plan_marker.ns = "arrow_place";
            plan_marker.action = visualization_msgs::Marker::ADD;
            plan_marker.id = 1;
            plan_marker.type = visualization_msgs::Marker::ARROW;
            plan_marker.scale.x = 0.2;
            plan_marker.scale.y = 0.08;
            plan_marker.scale.z = 0.08;
            plan_marker.color.r = 1.0;
            plan_marker.color.g = 0;
            plan_marker.color.b = 1.0;
            plan_marker.color.a = 0.5;
            plan_marker.pose = target_pose;
            tf2::Quaternion quat_tf, quat_rot, quat_new;
            tf2::convert(target_pose.orientation , quat_tf);
            quat_rot.setRPY(0, -M_PI/2, 0);
            quat_new = quat_tf * quat_rot;
            quat_new.normalize();
            // tf2::convert(quat_new , plan_marker.pose.orientation);
            plan_marker.pose.orientation = tf2::toMsg(quat_new);
            plan_marker_pub.publish(plan_marker);
            group.setPoseTarget(target_pose);
            moveit::planning_interface::MoveGroupInterface::Plan my_plan;
            moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
            if(success == moveit_msgs::MoveItErrorCodes::SUCCESS)
            {
                ROS_INFO("[wpv4_plan_server] 轨迹规划成功! 开始执行!");
                result_msg.data = "exec";
                result_pub.publish(result_msg);
                step = STEP_EXEC;
                // 机械臂按照规划的轨迹运动
                group.execute(my_plan);
                ROS_INFO("[wpv4_plan_server] 执行完成!");
                result_msg.data = "exec";
                result_pub.publish(result_msg);
                step = STEP_READY;
            }
            else
            {
                ROS_INFO("[wpv4_plan_server] 轨迹规划失败,请检查规划目标的参数");
                result_msg.data = "failed";
                result_pub.publish(result_msg);
                step = STEP_READY;
            }
            new_plan = false;
        }
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}
